/*	未完成.
	Rais_LineTracker: 巡线传感器组使用库(rebuilt) 
	Created: 2020.8.1
	by: Raiscies.

*/

//TODO: line 214 - tendency
//      line 168 - setPriority
//and so on...

#ifndef RAIS_LINETRACKER_H
#define RAIS_LINETRACKER_H

#include <Arduino.h>
#include "Rais_Graph.h"
#include "Rais_LibDef.h"
#include "Rais_LinkedList.h"

namespace rais{

class LineTracker;

class TrackerState{
	public:
	using event_t  = bool(*)(LineTracker&);
	using action_t = void(*)(LineTracker&);
	
	event_t event   = nullptr;
	action_t action = nullptr;
	
	TrackerState(){}
	TrackerState(event_t pEvent, action_t pAction): event(pEvent), action(pAction){}
	void init(event_t pEvent, action_t pAction){event = pEvent; action = pAction;}
	void operator() (LineTracker& tkr){action(tkr);}
	
	#define mkf_ptkr(name) [](LineTracker& name)
	
	//default state event functions.  (does not provide normal and stop state's default event)
	static bool dft_currentStateEvent(LineTracker& tkr){
		return tkr.triggingNum != 0;
	}
	static bool dft_inertiaStateEvent(LineTracker& tkr){
		return tkr.isStrongInertia ? (tkr.lastCorrectDeg == 0 ? false : true) : (&tkr.correctState == tkr.currentState ? true : false);
	}
	
	//default state action functions
	static void dft_normalStateAction(LineTracker& tkr){
		tkr.motion(tkr.power, 0);
	}
	static void dft_currentStateAction(LineTracker& tkr){
		tkr.motion(tkr.power, tkr.lastCorrectDeg = clamp(-90, tkr.tendency * tkr.correctWeight, 90));
	}
	static void dft_inertiaStateAction(LineTracker& tkr){
		tkr.motion(tkr.power, tkr.lastCorrectDeg);
	}
	static void dft_stopStateAction(LineTracker& tkr){
		tkr.motion(0, 0);
	}
	
};

class LineTracker{
	public:
	//default motion:
	using motionfp_t = void(*)(uint8_t, int16_t);
	motionfp_t motion = nullptr;
	
	
	//open control arguments:
	bool trigLevel       : 1;
	bool isRunning       : 1;
	bool isStrongInertia : 1;
	bool: 0;
	int16_t centerLine;
	uint8_t power = 100;
	uint8_t correctWeight = 9;
	
	friend class TrackerState;
	TrackerState normalState (nullptr,/*This must be set.*/        TrackerState::dft_normalStateAction ),
	             correctState(TrackerState::dft_currentStateEvent, TrackerState::dft_currentStateAction), 
	             inertiaState(TrackerState::dft_inertiaStateEvent, TrackerState::dft_inertiaStateAction), 
	             stopState   (nullptr,/*This could be ignored.*/   TrackerState::dft_stopStateAction   );
	
	protected:
	
	//control arguments:
	uint8_t sensorNum = 0;
	uint8_t* pins = nullptr;
	
	//state variables:
	bool* sens = nullptr;
	uint8_t triggingNum = 0;
	int8_t tendency = 0;
	int8_t lastCorrectDeg = 0;
	
	//running state control:
	uint8_t stateNum = 0;
	TrackerState*  currentState  = &normalState,
	            ** statePriority = nullptr;
	
	public:
	
	//main looping method:
	void run(){
		                                                           //updateDatas() returns whether the sensors' state is changed.
		if(not isRunning or currentState->action == nullptr or not updateDatas()) return;	//how funny is this condition!
		//setting and running current state:
		stateTransition().action(*this);
	}
	
	template <typename... PIN_Ts>
	LineTracker(PIN_Ts... pin_list){
		trigLevel = LOW;
		isRunning = false;
		isStrongInertia = false;
		
		setPins(pin_list...);
		setTrackerStates(normalState, correctState, inertiaState, stopState);
		
		//There are remained some works to do:
		//setting default motion;
		//setting normal state's event() method.
	}
	~LineTracker(){
		delete[] pins;
		delete[] sens;
		delete[] statePriority;
	}
	
	
	template <typename... PIN_Ts>
	void setPins(PIN_Ts... pin_list){
		if(sizeof...(pin_list) != sensorNum){
			sensorNum = sizeof...(pin_list);
			//it's no matter to delete nullptr:
			delete[] pins;
			delete[] sens;

			//to save sensor's pin numbers
			pins = new uint8_t[sensorNum];
			//to save sensor's electrical level
			sens = new bool[sensorNum];
		}
		
		setPins_recurion(0, pin_list...);
	}
	
	template <typename... STs>
	void setTrackerStates(STs&... stes){
		if(sizeof...(stes) != stateNum) {
			stateNum = sizeof...(stes);
			//it's no matter to delete nullptr:
			delete[] statePriority;
			statePriority = new TrackerState*[stateNum];
		}
		setTrackerStates_recursion(0, stes...);
	}
	
	bool    getSen(uint8_t idx) const{return sens[idx];}
	uint8_t getPin(uint8_t idx) const{return pins[idx];}
	uint8_t getSensorNum()      const{return sensorNum;}
	uint8_t getTriggingNum()    const{return triggingNum;}
	int8_t  getTendency()       const{return tendency;}
	int8_t  getLastCorrectDeg() const{return lastCorrectDeg;}
	uint8_t getStateNum()       const{return stateNum;}
	
	// void setPriority(uint8_t newPriority, TrackerState& ste){
		// newPriority >= stateNum ? newPriority = stateNum - 1 : 0;
		// uint8_t currentPriority = stateNum;
		// for(uint8_t i = 0; i < stateNum; i++){
			// if(statePriority[i] == &ste) {
				// currentPriority = i;
				// break;
			// }
		// }
		// if(stateNum == currentPriority) return;
		// for(uint8_t before = 0; before < currentPriority; before++){
			
		// }
	// }
	
	protected:
	
	//return whether the new sensors datas is different from old data
	bool updateDatas(){
		triggingNum = 0;
		bool isChanged = false;
		for(uint8_t i = 0; i < sensorNum; i++){
			//all this bit operations are just for remove branch statement :)
			bool newSenData = digitalRead(pins[i]) ^ trigLevel;	//whether this sensor is trigging
			isChanged |= newSenData ^ sens[i];					//set isChanged flag
			triggingNum += newSenData;							//count trigging sensor's number
			sens[i] = newSenData;								//update sensor datas
		}
		if(isChanged){
			//update tendency:
			tendency = 0;
			// for(uint8_t i = 1; i < sensorNum / 2 + 1; i++) tendency += (sens[sensorNum - i] - sens[i - 1]) * i;
			// tendency -= centerLine;
			
			uint8_t absCenterLine = (static_cast<int16_t>(sensorNum) + centerLine) / 2;
			if((centerLine ^ sensorNum) & 1) {
				//centerLine is in a sensor:
				//w means weight
				//counting right:
				for(uint8_t r = absCenterLine + 1, w = 1; r < sensorNum; r++, w++) tendency += sens[r] * w;
				//counting left:
				for(uint8_t l = 0, w = absCenterLine; l < absCenterLine; l++, w--) tendency -= sens[l] * w;
				
				}
			}else {
				//centerLine is not in a sansor:
				//TODO...
			}
			
			// int8_t absCenterLower = sensorNum / 2 + (sensorNum & 1 ? 1 : 0);
			// int8_t absCenterUpper = sensorNum / 2 + 1; //as 0.
			
			// if(sensorNum & 1){
				// sensorNum is odd(奇数):
				// for(uint8_t left = 0; left < c)
			// }else {
				// sensorNum is even(偶数):
			// }
			// for(uint8_t i = 0; i < sensorNum; i++){
			// }
			
		}
		return isChanged;
	}
	
	TrackerState& stateTransition(){
		for(uint8_t i = 0; i < stateNum - 1; i++){
			if(statePriority[i]->event != nullptr && statePriority[i]->event(*this)){
				currentState = statePriority[i];
				return;
			}
		}
		//do not test the last state's event
		return *(currentState = statePriority[stateNum - 1]);
	}

	template <typename... STs>
	void setTrackerStates_recursion(uint8_t thisPriority, TrackerState& thisState, STs&... stes){
		statePriority[thisPriority] = &thisState;
		setTrackerStates_recursion(thisPriority + 1, stes...);
	}
	void setTrackerStates_recursion(uint8_t thisPriority) const{unused(thisPriority);}
	
	template <typename... PIN_Ts>
	void setPins_recurion(uint8_t idx, uint8_t pin, PIN_Ts... pin_list){
		pinMode(pin, INPUT);
		pins[idx] = pin;
		setPins_recurion(idx + 1, pin_list...);
	}
	void setPins_recurion(uint8_t idx) const{unused(idx);}
	
	
};

class Traveller;

class ReachingVertexState: public TrackerState{
	
	static void dft_reachingVertexStateAction(LineTracker& tvr){
		
		
	}
};	


template <typename VT = Vertex>
class TravellerTask{
	public:
	using task_t = void(*)();
	
	VT* dest = nullptr;
	task_t task = nullptr;
	
	//task() 函数的调用方法:
	enum CallingMethodFlag: uint8_t{
		NEVER = 0,             //从不调用 task() 函数
		BEGIN_ONLY,            //仅在任务开始时调用一次   task() 函数
		END_ONLY,              //仅在到达目的地时调用一次 task() 函数
		BEGIN_AND_END,         //在任务开始和到达目的地时分别调用一次 task() 函数
		REACHED_VERTEX,	       //在每次到达节点时调用一次 task() 函数
		ALWAYS                 //在Traveller运行时不断调用 task() 函数
		
	} callingMethodFlag = BEGIN;
	
	TravellerTask(VT& pDest, task_t pTask = nullptr): dest(&pdest), task(pTask){}
	TravellerTask(task_t pTask = nullptr): task(pTask){}

};

template <typename VT = Vertex>
class Traveller: public LineTracker {
	public:
	using turningfp_t = bool(*)();	//turning functions' pointer, returns whether left the vertex.
	
	protected:

	PathFinder<VT>* guide = nullptr;
	uint8_t direction;
	uint8_t currentTurningDirection;
	
	turningfp_t* turningStrategy = nullptr;
	
	public:
	LinkedList<TravellerTask> tasks;
	
	LineTracker::TrackerState reachingVertexState;
	
	template <typename... PIN_Ts>
	Traveller(PathFinder<VT>& pGuide, uint8_t pDirection, PIN_Ts... pin_list): LineTracker(pin_list...){
		guide = &pGuide;
		direction = pDirection;
		if(guide->graph != nullptr) turningStrategy = new turningfp_t[guide->graph->getENum()];
		
		
		setTrackerStates(reachingVertexState, normalState, correctState, inertiaState, stopState);
		
	}
	~Traveller(){
		delete turningStrategy;
	}
	
	//Task System:
	template <typename T>
	void addTask(T&& task){tasks.push(task);}
	void addTargetTask(VT& v){tasks.push(TravellerTask(v));}
	void addTargetTask(venum_t v_idx){
		if(guide->graph != nullptr && !guide->graph->isError(v_idx))
			task.push(TravellerTask(guide->graph->getV(v_idx)));
	}
	
	
	
	protected:
	
	//check calling chain
	bool isCallingChainUnobstructed(venum_t v_idx){
		return guide->graph not_eq nullptr and not guide->graph->isError(v_idx);
	}
	
};

}


#endif